#include "Agent.h"

#include <cmath>
#include <algorithm>

#include "Definitions.h"
#include "KdTree.h"

namespace RVO
{
	/**
	 * \brief   A sufficiently small positive number.
	 */
	const float RVO_EPSILON = 0.00001f;

	/**
	 * \brief   Defines a directed line.
	 */
	class Line
	{
	public:
		/**
		 * \brief   The direction of the directed line.
		 */
		Vector3 direction;

		/**
		 * \brief   A point on the directed line.
		 */
		Vector3 point;
	};

	/**
	 * \brief   Solves a one-dimensional linear program on a specified line subject to linear constraints defined by planes and a spherical constraint.
	 * \param   planes        Planes defining the linear constraints.
	 * \param   planeNo       The plane on which the line lies.
	 * \param   line          The line on which the 1-d linear program is solved
	 * \param   radius        The radius of the spherical constraint.
	 * \param   optVelocity   The optimization velocity.
	 * \param   directionOpt  True if the direction should be optimized.
	 * \param   result        A reference to the result of the linear program.
	 * \return  True if successful.
	 */
	bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);

	/**
	 * \brief   Solves a two-dimensional linear program on a specified plane subject to linear constraints defined by planes and a spherical constraint.
	 * \param   planes        Planes defining the linear constraints.
	 * \param   planeNo       The plane on which the 2-d linear program is solved
	 * \param   radius        The radius of the spherical constraint.
	 * \param   optVelocity   The optimization velocity.
	 * \param   directionOpt  True if the direction should be optimized.
	 * \param   result        A reference to the result of the linear program.
	 * \return  True if successful.
	 */
	bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);

	/**
	 * \brief   Solves a three-dimensional linear program subject to linear constraints defined by planes and a spherical constraint.
	 * \param   planes        Planes defining the linear constraints.
	 * \param   radius        The radius of the spherical constraint.
	 * \param   optVelocity   The optimization velocity.
	 * \param   directionOpt  True if the direction should be optimized.
	 * \param   result        A reference to the result of the linear program.
	 * \return  The number of the plane it fails on, and the number of planes if successful.
	 */
	size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);

	/**
	 * \brief   Solves a four-dimensional linear program subject to linear constraints defined by planes and a spherical constraint.
	 * \param   planes     Planes defining the linear constraints.
	 * \param   beginPlane The plane on which the 3-d linear program failed.
	 * \param   radius     The radius of the spherical constraint.
	 * \param   result     A reference to the result of the linear program.
	 */
	void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result);

	Agent::Agent(RVOSimulator *sim) : sim_(sim), id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f) {}

   //用到neighborDist
	void Agent::computeNeighbors()
	{
		agentNeighbors_.clear(); // 清空邻居列表

		if (maxNeighbors_ > 0)
		{										   // 检查最大邻居数量是否大于0
            // 以下这个函数就是在更改agentNeighbors_
			sim_->kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_); // 计算并更新邻居列表
		}
	}

	// 它使用ORCA（Optimal Reciprocal Collision Avoidance）方法来计算智能体之间的避碰行为
	void Agent::computeNewVelocity()
	{
		orcaPlanes_.clear();				  // 清空ORCA平面列表
		const float invTimeHorizon = 1.0f / timeHorizon_; // 计算时间视野的倒数

		/* 创建智能体的ORCA平面 */
		for (size_t i = 0; i < agentNeighbors_.size(); ++i)
		{								       // 遍历每个邻居智能体
			const Agent *const other = agentNeighbors_[i].second;	       // 获取邻居智能体指针
			//这里的position_是在rvo->updateState添加的当前agent的位置
			// 改这块就好了===============================
			const Vector3 relativePosition = other->position_ - position_; // 计算相对位置
			const Vector3 relativeVelocity = velocity_ - other->velocity_; // 计算相对速度
			// const Vector3 relativePosition = relative_position_; // 计算相对位置
			// const Vector3 relativeVelocity = relative_velocity_; // 计算相对速度

			const float distSq = absSq(relativePosition);		       // 计算相对位置的平方距离
			const float combinedRadius = radius_ + other->radius_;	       // 计算合并半径
			const float combinedRadiusSq = sqr(combinedRadius);	       // 计算合并半径的平方

			Plane plane; // 定义一个平面对象
			Vector3 u;   // 定义速度调整向量

			if (distSq > combinedRadiusSq)
			{										// 如果没有发生碰撞
				const Vector3 w = relativeVelocity - invTimeHorizon * relativePosition; // 计算从截断中心到相对速度的向量
				const float wLengthSq = absSq(w);					// 计算w向量的平方长度

				const float dotProduct = w * relativePosition; // 计算w向量和相对位置的点积

				if (dotProduct < 0.0f && sqr(dotProduct) > combinedRadiusSq * wLengthSq)
				{						    // 如果投影在截断圆上
					const float wLength = std::sqrt(wLengthSq); // 计算w向量的长度
					const Vector3 unitW = w / wLength;	    // 计算w向量的单位向量

					plane.normal = unitW;					 // 设置平面的法向量
					u = (combinedRadius * invTimeHorizon - wLength) * unitW; // 计算速度调整向量
				}
				else
				{																  // 如果投影在圆锥上
					const float a = distSq;													  // 设置系数a
					const float b = relativePosition * relativeVelocity;									  // 设置系数b
					const float c = absSq(relativeVelocity) - absSq(cross(relativePosition, relativeVelocity)) / (distSq - combinedRadiusSq); // 设置系数c
					const float t = (b + std::sqrt(sqr(b) - a * c)) / a;									  // 计算t值
					const Vector3 w = relativeVelocity - t * relativePosition;								  // 计算w向量
					const float wLength = abs(w);												  // 计算w向量的长度
					const Vector3 unitW = w / wLength;											  // 计算w向量的单位向量

					plane.normal = unitW;			    // 设置平面的法向量
					u = (combinedRadius * t - wLength) * unitW; // 计算速度调整向量
				}
			}
			else
			{									     // 如果发生碰撞
				const float invTimeStep = 1.0f / sim_->timeStep_;		     // 计算时间步长的倒数
				const Vector3 w = relativeVelocity - invTimeStep * relativePosition; // 计算w向量
				const float wLength = abs(w);					     // 计算w向量的长度
				const Vector3 unitW = w / wLength;				     // 计算w向量的单位向量

				plane.normal = unitW;				      // 设置平面的法向量
				u = (combinedRadius * invTimeStep - wLength) * unitW; // 计算速度调整向量
			}
			
			// 有多少个neighbor，就有多少个orca平面
			plane.point = velocity_ + 0.5f * u; // 计算平面上的点
			orcaPlanes_.push_back(plane);	    // 将平面添加到ORCA平面列表中
		}

		const size_t planeFail = linearProgram3(orcaPlanes_, maxSpeed_, prefVelocity_, false, newVelocity_); // 计算新的速度，如果失败返回失败的平面索引

		if (planeFail < orcaPlanes_.size())
		{									 // 如果存在失败的平面
			linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_); // 调用备用算法处理失败的平面
		}
	}

	// 用于在智能体的邻居列表中插入一个邻居智能体，并确保邻居列表按距离排序且不超过最大邻居数量。
	void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq)
	{
		if (this != agent) // 检查是否尝试将自身添加为邻居
		{
			const float distSq = absSq(position_ - agent->position_); // 计算当前智能体与目标智能体之间的平方距离
			// const float distSq = absSq(relative_position_);

			if (distSq < rangeSq) // 如果计算得到的距离小于给定的范围
			{
				if (agentNeighbors_.size() < maxNeighbors_) // 如果邻居列表的大小小于最大邻居数量
				{
					agentNeighbors_.push_back(std::make_pair(distSq, agent)); // 将新的邻居添加到邻居列表中
				}

				size_t i = agentNeighbors_.size() - 1; // 获取新添加的邻居的索引

				// 将新邻居插入到已排序的邻居列表中，以保持列表的有序性
				while (i != 0 && distSq < agentNeighbors_[i - 1].first)
				{
					agentNeighbors_[i] = agentNeighbors_[i - 1]; // 将更远的邻居向后移动
					--i;					     // 向前移动
				}

				agentNeighbors_[i] = std::make_pair(distSq, agent); // 将新的邻居插入到正确的位置

				if (agentNeighbors_.size() == maxNeighbors_) // 如果邻居列表已达到最大数量
				{
					rangeSq = agentNeighbors_.back().first; // 更新范围，使其为最远的邻居的距离
				}
			}
		}
	}
	// 用于更新智能体的位置和速度。
	void Agent::update()
	{
		velocity_ = newVelocity_;		  // 更新智能体的速度为计算得到的新速度
		position_ += velocity_ * sim_->timeStep_; // 根据速度和时间步长更新智能体的位置
	}

	// 用于在一个圆形区域内（以给定的半径为界限）找到一条线的交点。该函数的主要目的是在避碰算法中优化智能体的速度，使其在一个允许的范围内运行。
	bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
	{
		const float dotProduct = line.point * line.direction;			      // 计算点和方向的点积
		const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(line.point); // 计算判别式，用于判断交点是否在圆形区域内

		if (discriminant < 0.0f)
		{
			/* 最大速度球完全使线无效。 */
			return false; // 如果判别式小于0，表示没有交点，返回false
		}

		const float sqrtDiscriminant = std::sqrt(discriminant); // 计算判别式的平方根
		float tLeft = -dotProduct - sqrtDiscriminant;		// 计算t的左边界
		float tRight = -dotProduct + sqrtDiscriminant;		// 计算t的右边界

		for (size_t i = 0; i < planeNo; ++i)
		{
			const float numerator = (planes[i].point - line.point) * planes[i].normal; // 计算分子
			const float denominator = line.direction * planes[i].normal;		   // 计算分母

			if (sqr(denominator) <= RVO_EPSILON)
			{
				/* 线几乎与平面i平行。 */
				if (numerator > 0.0f)
				{
					return false; // 如果分子大于0，返回false，表示无解
				}
				else
				{
					continue; // 否则继续下一个平面
				}
			}

			const float t = numerator / denominator; // 计算t值

			if (denominator >= 0.0f)
			{
				/* 平面i限制线的左边界。 */
				tLeft = std::max(tLeft, t); // 更新t的左边界
			}
			else
			{
				/* 平面i限制线的右边界。 */
				tRight = std::min(tRight, t); // 更新t的右边界
			}

			if (tLeft > tRight)
			{
				return false; // 如果左边界超过右边界，返回false，表示无解
			}
		}

		if (directionOpt)
		{
			/* 优化方向。 */
			if (optVelocity * line.direction > 0.0f)
			{
				/* 取右边界。 */
				result = line.point + tRight * line.direction; // 如果方向优化，则选择tRight作为结果
			}
			else
			{
				/* 取左边界。 */
				result = line.point + tLeft * line.direction; // 否则选择tLeft作为结果
			}
		}
		else
		{
			/* 优化最近点。 */
			const float t = line.direction * (optVelocity - line.point); // 计算最优t值

			if (t < tLeft)
			{
				result = line.point + tLeft * line.direction; // 如果t小于左边界，选择tLeft作为结果
			}
			else if (t > tRight)
			{
				result = line.point + tRight * line.direction; // 如果t大于右边界，选择tRight作为结果
			}
			else
			{
				result = line.point + t * line.direction; // 否则选择t作为结果
			}
		}

		return true; // 返回true，表示找到了解
	}

	// 用于计算满足给定约束的速度向量。这个函数的主要目的是在智能体的避碰算法中，在一个半径为radius的球体内找到一个速度向量，使其尽可能接近给定的最优速度optVelocity，同时满足所有给定的平面约束。
	bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
	{
		const float planeDist = planes[planeNo].point * planes[planeNo].normal; // 计算平面与原点的距离
		const float planeDistSq = sqr(planeDist);				// 计算距离的平方
		const float radiusSq = sqr(radius);					// 计算半径的平方

		if (planeDistSq > radiusSq)
		{
			/* 最大速度球完全使平面 planeNo 无效。 */
			return false; // 如果平面距离的平方大于半径的平方，则返回false，表示无解
		}

		const float planeRadiusSq = radiusSq - planeDistSq;		// 计算平面上的圆的半径的平方
		const Vector3 planeCenter = planeDist * planes[planeNo].normal; // 计算平面中心

		if (directionOpt)
		{
			/* 投影方向 optVelocity 到平面 planeNo 上。 */
			const Vector3 planeOptVelocity = optVelocity - (optVelocity * planes[planeNo].normal) * planes[planeNo].normal; // 计算平面上的最优速度
			const float planeOptVelocityLengthSq = absSq(planeOptVelocity);							// 计算平面上最优速度的平方长度

			if (planeOptVelocityLengthSq <= RVO_EPSILON)
			{
				result = planeCenter; // 如果最优速度的平方长度小于一个很小的值，则结果为平面中心
			}
			else
			{
				result = planeCenter + std::sqrt(planeRadiusSq / planeOptVelocityLengthSq) * planeOptVelocity; // 否则，计算结果为平面中心加上最优速度在平面上的投影
			}
		}
		else
		{
			/* 投影点 optVelocity 到平面 planeNo 上。 */
			result = optVelocity + ((planes[planeNo].point - optVelocity) * planes[planeNo].normal) * planes[planeNo].normal; // 计算点在平面上的投影

			/* 如果在平面圆外，则投影到平面圆上。 */
			if (absSq(result) > radiusSq)
			{
				const Vector3 planeResult = result - planeCenter;				     // 计算结果相对于平面中心的向量
				const float planeResultLengthSq = absSq(planeResult);				     // 计算该向量的平方长度
				result = planeCenter + std::sqrt(planeRadiusSq / planeResultLengthSq) * planeResult; // 将结果投影到平面圆上
			}
		}

		for (size_t i = 0; i < planeNo; ++i)
		{
			if (planes[i].normal * (planes[i].point - result) > 0.0f)
			{
				/* 结果不满足约束 i，计算新的最优结果。 */
				/* 计算平面 i 和平面 planeNo 的交线。 */
				Vector3 crossProduct = cross(planes[i].normal, planes[planeNo].normal); // 计算两个平面的法向量的叉积

				if (absSq(crossProduct) <= RVO_EPSILON)
				{
					/* 平面 planeNo 和 i（几乎）平行，平面 i 完全使平面 planeNo 无效。 */
					return false; // 如果叉积的平方小于一个很小的值，表示平面几乎平行，返回false
				}

				Line line;
				line.direction = normalize(crossProduct);													      // 规范化交线方向
				const Vector3 lineNormal = cross(line.direction, planes[planeNo].normal);									      // 计算交线的法向量
				line.point = planes[planeNo].point + (((planes[i].point - planes[planeNo].point) * planes[i].normal) / (lineNormal * planes[i].normal)) * lineNormal; // 计算交线上的点

				if (!linearProgram1(planes, i, line, radius, optVelocity, directionOpt, result))
				{
					return false; // 如果 linearProgram1 返回 false，表示无解，返回 false
				}
			}
		}

		return true; // 返回 true，表示找到了解
	}

	// 用于计算满足给定约束（平面）的最优速度向量。函数尝试找到一个在半径为radius的球体内的速度向量，使其尽可能接近给定的最优速度optVelocity，同时满足所有给定的平面约束。如果无法满足某个约束，则返回该约束的索引。
	size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
	{
		if (directionOpt)
		{
			/* 优化方向。注意，在这种情况下，优化速度是单位长度。 */
			result = optVelocity * radius; // 如果优化方向，将最优速度扩展到给定的半径
		}
		else if (absSq(optVelocity) > sqr(radius))
		{
			/* 优化最近点并且在圆外。 */
			result = normalize(optVelocity) * radius; // 如果最优速度的平方长度大于半径的平方，将其归一化并扩展到给定的半径
		}
		else
		{
			/* 优化最近点并且在圆内。 */
			result = optVelocity; // 如果最优速度的平方长度小于或等于半径的平方，直接使用最优速度
		}

		for (size_t i = 0; i < planes.size(); ++i)
		{
			if (planes[i].normal * (planes[i].point - result) > 0.0f)
			{
				/* 结果不满足约束 i，计算新的最优结果。 */
				const Vector3 tempResult = result; // 保存当前结果

				if (!linearProgram2(planes, i, radius, optVelocity, directionOpt, result))
				{
					result = tempResult; // 如果 linearProgram2 返回 false，恢复之前的结果
					return i;	     // 返回不满足的平面的索引
				}
			}
		}

		return planes.size(); // 如果所有约束都满足，返回 planes.size()
	}

	// 用于在给定约束（平面）下计算一个满足所有约束的速度向量。这个函数的主要目的是处理linearProgram3无法解决的约束冲突。具体实现是通过逐步调整结果向量，使其满足所有平面的约束条件。
	void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result)
	{
		float distance = 0.0f; // 初始化距离为0

		for (size_t i = beginPlane; i < planes.size(); ++i)
		{
			if (planes[i].normal * (planes[i].point - result) > distance)
			{
				/* 结果不满足平面 i 的约束。 */
				std::vector<Plane> projPlanes; // 用于存储投影平面

				for (size_t j = 0; j < i; ++j)
				{
					Plane plane;

					const Vector3 crossProduct = cross(planes[j].normal, planes[i].normal); // 计算两个平面的法向量的叉积

					if (absSq(crossProduct) <= RVO_EPSILON)
					{
						/* 平面 i 和平面 j 几乎平行。 */
						if (planes[i].normal * planes[j].normal > 0.0f)
						{
							/* 平面 i 和平面 j 指向相同方向。 */
							continue;
						}
						else
						{
							/* 平面 i 和平面 j 指向相反方向。 */
							plane.point = 0.5f * (planes[i].point + planes[j].point); // 平面点是两个平面点的中点
						}
					}
					else
					{
						/* 平面点是平面 i 和平面 j 的交线上的点。 */
						const Vector3 lineNormal = cross(crossProduct, planes[i].normal);									   // 计算交线的法向量
						plane.point = planes[i].point + (((planes[j].point - planes[i].point) * planes[j].normal) / (lineNormal * planes[j].normal)) * lineNormal; // 计算交点
					}

					plane.normal = normalize(planes[j].normal - planes[i].normal); // 计算投影平面的法向量并归一化
					projPlanes.push_back(plane);				       // 将投影平面添加到列表中
				}

				const Vector3 tempResult = result; // 保存当前结果

				if (linearProgram3(projPlanes, radius, planes[i].normal, true, result) < projPlanes.size())
				{
					/* 原则上不应该发生。这是因为结果已经在这个线性规划的可行区域内。如果失败，是由于小的浮点误差，并保持当前结果。 */
					result = tempResult;
				}

				distance = planes[i].normal * (planes[i].point - result); // 更新距离
			}
		}
	}

}
